#include <stdio.h>

#include <unistd.h>

#include "ohos_init.h"
#include "cmsis_os2.h"

#include <iot_pwm.h>
#include <iot_gpio.h>

#include <unistd.h>
#include <hi_types_base.h>
#include <hi_early_debug.h>

#include <hi_pwm.h>
#include <hi_gpio.h>
#include <hi_io.h>
#include "car_test.h"

#include "iot_pwm.h"
//最下的，
//电机1 正反转的IO口
#define DJ_1_GPIOA 0
#define DJ_1_GPIOB 1

//电机2 正反转的IO口
#define DJ_2_GPIOA 9
#define DJ_2_GPIOB 10

//最上面的
#define DOUBLE_ELECTRIC_MACHINERY
#if defined(DOUBLE_ELECTRIC_MACHINERY)	
//电机1 正反转的IO口
#define DJ_3_GPIOA 6
#define DJ_3_GPIOB 8
//电机2 正反转的IO口
#define DJ_4_GPIOA 13
#define DJ_4_GPIOB 14
#endif

#ifdef CAR_DISPLAY_CMD
extern void car_cmd_test(char* str);
#endif



void gpio_control(unsigned int  gpio, IotGpioValue value) {
    //控制IO口输出
	hi_io_set_func(gpio,0);
#if defined(DOUBLE_ELECTRIC_MACHINERY)	
	hi_io_set_func(13,4);
	hi_io_set_func(14,4);
#endif
	IoTGpioSetDir(gpio,IOT_GPIO_DIR_OUT);
	IoTGpioSetOutputVal(gpio,value);
}

struct car_sys_info car_info;

//CarStatus carstatus = CAR_STATUS_STOP;
//CarMode carmode = CAR_MODE_STEP;

void car_info_init(void)
{
	car_info.go_status = CAR_STATUS_STOP;
	car_info.cur_status = CAR_STATUS_STOP;

	car_info.mode = CAR_MODE_STEP;
	car_info.protocol = COMM_PROTOCOL_UDP;

	car_info.step_count = CAR_STEP_COUNT;
}

void step_count_update(void)
{
	car_info.step_count = CAR_STEP_COUNT;
}

void set_car_status(CarStatus status)
{
	if(status != car_info.cur_status)
	{
		car_info.status_change = 1;
	}
	car_info.go_status = status;

	step_count_update();
}


//设置行驶模式
void set_car_mode(CarMode mode)
{
	car_info.mode = mode;
}

//设置通信协议
void set_comm_mode(ProtoStatus mode)
{
	car_info.protocol = mode;
}


void car_gpio_init(void)
{
    //IO口初始化
	IoTGpioInit(DJ_1_GPIOA);
	IoTGpioInit(DJ_1_GPIOB);
	IoTGpioInit(DJ_2_GPIOA);
	IoTGpioInit(DJ_2_GPIOB);
#if defined(DOUBLE_ELECTRIC_MACHINERY)	
	IoTGpioInit(DJ_3_GPIOA);
	IoTGpioInit(DJ_3_GPIOB);
	IoTGpioInit(DJ_4_GPIOA);
	IoTGpioInit(DJ_4_GPIOB);
#endif	
	printf("car_gpio_init \r\n");
}

//停止
void car_gpio_stop(void)
{
#if defined(DOUBLE_ELECTRIC_MACHINERY)	
	gpio_control(DJ_3_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_3_GPIOB,IOT_GPIO_VALUE1);
	gpio_control(DJ_4_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_4_GPIOB,IOT_GPIO_VALUE1);
#else
	gpio_control(DJ_1_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_1_GPIOB,IOT_GPIO_VALUE1);
	gpio_control(DJ_2_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_2_GPIOB,IOT_GPIO_VALUE1);
#endif	

}
void car_stop(void)
{
	car_info.cur_status = car_info.go_status;

	printf("car_gpio_stop \r\n");

	car_gpio_stop();
}

//前进
void car_gpio_forward(void)
{
#if defined(DOUBLE_ELECTRIC_MACHINERY)	
    gpio_control(DJ_1_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_1_GPIOB,IOT_GPIO_VALUE1);
	gpio_control(DJ_2_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_2_GPIOB,IOT_GPIO_VALUE0);
	gpio_control(DJ_3_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_3_GPIOB,IOT_GPIO_VALUE1);
	gpio_control(DJ_4_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_4_GPIOB,IOT_GPIO_VALUE0);
#else
    gpio_control(DJ_1_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_1_GPIOB,IOT_GPIO_VALUE0);
	gpio_control(DJ_2_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_2_GPIOB,IOT_GPIO_VALUE0);
#endif		
}
void car_forward(void)
{
	if(car_info.go_status != CAR_STATUS_FORWARD)
	{
		//直接退?
		return ;
	}
	if(car_info.cur_status == car_info.go_status)
	{
		//状态没有变化，直接推出
		return;
	}

	car_info.cur_status = car_info.go_status;

	printf("car_gpio_forward \r\n");

	car_gpio_forward();

	step_count_update();
}

//后退
void car_gpio_backward(void)
{
#if defined(DOUBLE_ELECTRIC_MACHINERY)
    gpio_control(DJ_1_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_1_GPIOB,IOT_GPIO_VALUE0);
	gpio_control(DJ_2_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_2_GPIOB,IOT_GPIO_VALUE1);	
	gpio_control(DJ_3_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_3_GPIOB,IOT_GPIO_VALUE0);
	gpio_control(DJ_4_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_4_GPIOB,IOT_GPIO_VALUE1);
#else
  	gpio_control(DJ_1_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_1_GPIOB,IOT_GPIO_VALUE1);
	gpio_control(DJ_2_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_2_GPIOB,IOT_GPIO_VALUE1);
#endif		
}
void car_backward(void)
{
	if(car_info.go_status != CAR_STATUS_BACKWARD)
	{
		//直接退?
		return ;
	}
	if(car_info.cur_status == car_info.go_status)
	{
		//状态没有变化，直接推出
		return;
	}

	car_info.cur_status = car_info.go_status;

	printf("car_gpio_backward \r\n");

	car_gpio_backward();

	step_count_update();
}

//左转
void car_gpio_left(void)
{
#if defined(DOUBLE_ELECTRIC_MACHINERY)	
    gpio_control(DJ_1_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_1_GPIOB,IOT_GPIO_VALUE1);
	gpio_control(DJ_2_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_2_GPIOB,IOT_GPIO_VALUE0);
	gpio_control(DJ_3_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_3_GPIOB,IOT_GPIO_VALUE1);
	gpio_control(DJ_4_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_4_GPIOB,IOT_GPIO_VALUE0);
#else
	gpio_control(DJ_1_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_1_GPIOB,IOT_GPIO_VALUE0);
	gpio_control(DJ_2_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_2_GPIOB,IOT_GPIO_VALUE0);
#endif	
}
void car_left(void)
{
	if(car_info.go_status != CAR_STATUS_LEFT)
	{
		//直接退出?
		return ;
	}
	if(car_info.cur_status == car_info.go_status)
	{
		//状态没有变化，直接推出
		return;
	}

	car_info.cur_status = car_info.go_status;

	printf("car_gpio_left \r\n");

	car_gpio_left();

	step_count_update();
}

//右转
void car_gpio_right(void)
{
#if defined(DOUBLE_ELECTRIC_MACHINERY)	
    gpio_control(DJ_1_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_1_GPIOB,IOT_GPIO_VALUE0);
	gpio_control(DJ_2_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_2_GPIOB,IOT_GPIO_VALUE0);
	gpio_control(DJ_3_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_3_GPIOB,IOT_GPIO_VALUE0);
	gpio_control(DJ_4_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_4_GPIOB,IOT_GPIO_VALUE0);
#else
	gpio_control(DJ_1_GPIOA,IOT_GPIO_VALUE1);
	gpio_control(DJ_1_GPIOB,IOT_GPIO_VALUE0);
	gpio_control(DJ_2_GPIOA,IOT_GPIO_VALUE0);
	gpio_control(DJ_2_GPIOB,IOT_GPIO_VALUE0);
#endif			
}
void car_right(void)
{
	if(car_info.go_status != CAR_STATUS_RIGHT)
	{
		//直接退出?
		return ;
	}
	if(car_info.cur_status == car_info.go_status)
	{
		//状态没有变化，直接推出
		return;
	}

	car_info.cur_status = car_info.go_status;

	printf("car_gpio_right \r\n");

	car_gpio_right();

	step_count_update();
}

extern void start_tcp_thread(void);
extern void start_udp_thread(void);
extern void cancel_tcp_thread(void);
extern void cancel_udp_thread(void);

void car_test(void)
{
    car_gpio_init();
	car_info_init();
	//set_car_status(CAR_STATUS_FORWARD);
	//set_car_mode(CAR_MODE_ALWAY);

	car_forward();
	sleep(1);

	car_backward();
	sleep(1);

	car_right();
	sleep(1);

	car_left();
	sleep(1);

	car_stop();

	while(1)
	{
		if(car_info.status_change)
		{
			car_info.status_change = 0;

			switch(car_info.go_status)
			{
				case CAR_STATUS_STOP:
					car_stop();
					break;
				
				case CAR_STATUS_FORWARD:
					car_forward();
					break;

				case CAR_STATUS_BACKWARD:
					car_backward();
					break;

				case CAR_STATUS_LEFT:
					car_left();
					break;

				case CAR_STATUS_RIGHT:
					car_right();
					break;

				default:

					break;
			}
		}
		

		if(car_info.mode == CAR_MODE_STEP)
		{
			if(car_info.go_status != CAR_STATUS_STOP)
			{
				if(car_info.step_count > 0)
				{
					car_info.step_count --;
					printf("car_info.step_count=%d \r\n",car_info.step_count);
				}else{
					printf("stop... \r\n");
					set_car_status(CAR_STATUS_STOP);
				}
			}
		}
		
		if (car_info.protocol == COMM_PROTOCOL_TCP)
		{
			cancel_udp_thread();
			printf("======== 【UDP】 Stop Protocol========\r\n");
			start_tcp_thread();
			printf("======== 【TCP】 Start Protocol ========\r\n");
			car_info.protocol = COMM_PROTOCOL_NOP;
		}
		else if (car_info.protocol == COMM_PROTOCOL_UDP)
		{
			cancel_tcp_thread();
			printf("======== 【TCP】 Stop Protocol ========\r\n");
			start_udp_thread();
			printf("======== 【UDP】 Start Protocol ========\r\n");
			car_info.protocol = COMM_PROTOCOL_NOP;
		}

		usleep(1000);
	}
	
}

